syms x1 x2 x3;
f1 = -2*x1+x2 - x3;
f2 = x1 - x2 -x2^3 + x3*x3;
J=jacobian([f1;f2],[x1;x2;x3])
%%
clear clc
syms x y k u
eqns = [x + y == k, x - y == u];
vars = [x y];
[x, y] = solve(eqns, vars)
%%
clc,clear
syms g
syms R L Lm Ll Mw Mp M Iw Ip Im
syms x the fha T Tp Nf

syms dx dt df

syms ddx ddt ddf




% ddx = (T-N*R)

% Nm Pm N  P  中间变量

% 式(1.7)
Nm = M*(ddx + (L+Lm )*cos(the)*ddt  - (L+Lm )*sin(the)*dt^2  - Ll*ddf*cos(fha) + Ll*sin(fha)*df^2 );

% 式(1.8)
Pm = M*g - M*( (L+Lm )*cos(the)*dt^2  + (L+Lm )*sin(the)*ddt  + Ll*ddf*sin(fha) + Ll*cos(fha)*df^2);

% 式(1.4)
N = Nm + Mp*( ddx + L*ddt*cos(the) - L*sin(the)*dt^2);

% 式(1.5)
P = Pm + Mp*g + Mp*( -L*ddt*sin(the) - L*cos(the)*dt^2 );

% 计算
% 式(1.3) (1.6)  (1.9)
eqns = [
    ddx == (T-N*R)/(Iw/R + Mw*R), 
    Ip*ddt == (P*L+Pm*Lm)*sin(the) - (N*L+Nm*Lm)*cos(the) - T + Tp,
    Im*ddf == Tp+Nm*Ll*cos(fha) + Pm*Ll*sin(fha)
    ];
vars = [ddt ddx ddf];
[SoVddt, SoVddx, SoVddf] = solve(eqns, vars);

%% 雅可比矩阵
J=jacobian([SoVddt;SoVddx;SoVddf],[the; dt; x; dx; fha; df]);
% 带入平衡点
J_ =  subs(J,{the,fha,dx,dt,df},{0,0,0,0,0});%%%%代入数值
%% 雅可比矩阵
J=jacobian([SoVddt;SoVddx;SoVddf],[the; x; fha; T ; Tp]);
% 带入平衡点
J_ =  subs(J,{the,fha,dx,dt,df},{0,0,0,0,0});%%%%代入数值
simplify(J_)
%% 测试数据
Nm_test = subs(Nm,{the,fha,R, L, Lm,Ll, Mw, Mp, M, Iw, Ip, Im,},{0,0,1,1,1,1,1,1,1,1,1,1})%%%%代入数值
N_test = subs(N,{the,fha,R, L, Lm,Ll, Mw, Mp, M, Iw, Ip, Im,},{0,0,1,1,1,1,1,1,1,1,1,1})%%%%代入数值
Pm_test = subs(Pm,{the,fha,R, L, Lm,Ll, Mw, Mp, M, Iw, Ip, Im,},{0,0,1,1,1,1,1,1,1,1,1,1})%%%%代入数值
P_test = subs(P,{the,fha,R, L, Lm,Ll, Mw, Mp, M, Iw, Ip, Im,},{0,0,1,1,1,1,1,1,1,1,1,1})%%%%代入数值

%% 测试计算结果是否正确


ddx_test = subs(SoVddx,{the,fha,R, L, Lm,Ll, Mw, Mp, M, Iw, Ip, Im,},{0,0,1,1,1,1,1,1,1,1,1,1});%%%%代入数值
ddf_test = subs(SoVddf,{the,fha,R, L, Lm,Ll, Mw, Mp, M, Iw, Ip, Im,},{0,0,1,1,1,1,1,1,1,1,1,1});%%%%代入数值
ddt_test = subs(SoVddt,{the,fha,R, L, Lm,Ll, Mw, Mp, M, Iw, Ip, Im,},{0,0,1,1,1,1,1,1,1,1,1,1});%%%%代入数值


%%
syms ddx ddt ddf T Tp
eqns = [4*ddx== T-3*ddt+ddf,6*ddt == 2*ddf-3*ddx-T + Tp, ddf == Tp + 2*ddt-ddf + ddx];
vars = [ddt ddx ddf];
[SoVddt1 SoVddx1 SoVddf1] = solve(eqns, vars)